package org.scalpel.mainApps.HHObjects;

import java.util.ArrayList;

import ioio.lib.api.IOIO;
import ioio.lib.api.TwiMaster;
import ioio.lib.api.exception.ConnectionLostException;

import org.opencv.core.Mat;
import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;
import org.scalpel.mainApps.GenericVirtualMotor;
import org.scalpel.mainApps.IoioMotorControler;
import org.scalpel.mainApps.Mobile;
//import org.hh.locodo.IoioMotorControlerWithoutEncoder;

import android.content.Context;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.Log;
import android.view.MotionEvent;

/**
 * 
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * ModLoco is the main locomotion class. 
 * It instanciate : 
 * - MotorControlers how are send to Locodo.Locomotion class.
 * - Virtual MotorControlers in the simulation mode.
 * - Mobile : which display the robot on the view
 * - Locodo.Locomotion which compute current positions and current speeds
 * 
 * ModLoco could be instantiate from the worldmap xml file.
 *
 */
public class ModLoco extends HHObject {

  // MotorControler (send pwm, read encoder value, and compute automatic speed regulator) 
  private IoioMotorControler fMotorLeft;
  private IoioMotorControler fMotorRight;

  // Generic motor without hardware connection
  private GenericVirtualMotor fMotorVirtualLeft;
  private GenericVirtualMotor fMotorVirtualRight;
  
  // Locomotion Engine to compute pid and field position 
  private Mobile fLocoEngine;
  
  // Conversion 
  double fConvPixToMeter;
  double fConvMeterToPix;
  
  // Geometric robot description
  double fWheelDistanceToCenter;
  double fWheelDiameter;
  double fEncoderWheelPerimeter;
  int fEncoderTickPerRotation;
  double fMaxRealSpeedinTickPerSecond;
  
  boolean fLastTargetReached = false;
    
  // IOIO variable
  private boolean fIoioInit = false;
  TwiMaster fTwi;
  
  
  public ModLoco(String name, 
      double startposx, double startposy, double startposa, 
      double encoderWheelPerimeter,
      double encoderWheelsDistanceToCenter, int encoderTickPerRotation, 
      double maxRealSpeedinTickPerSecond, 
      double convMeterToPix) {
    super(name);
    
    // Store geometric values
    fConvMeterToPix = convMeterToPix;
    fConvPixToMeter = 1./fConvMeterToPix;
    fEncoderWheelPerimeter = encoderWheelPerimeter;
    fWheelDiameter = fEncoderWheelPerimeter / Math.PI;
    fWheelDistanceToCenter = encoderWheelsDistanceToCenter;
    fEncoderTickPerRotation = encoderTickPerRotation;
    fMaxRealSpeedinTickPerSecond = maxRealSpeedinTickPerSecond;

    // Define the shape of the robot
    ArrayList<KeyPoint> shape = new ArrayList<KeyPoint>();
    shape.add(new KeyPoint(-0.15, -0.15));
    shape.add(new KeyPoint(-0.10, -0.13));
    shape.add(new KeyPoint( 0.10, -0.13));
    shape.add(new KeyPoint( 0.15, -0.15));
    shape.add(new KeyPoint( 0.13, -0.10));
    shape.add(new KeyPoint( 0.13,  0.10));
    shape.add(new KeyPoint( 0.15,  0.15));
    shape.add(new KeyPoint( 0.10,  0.13));
    shape.add(new KeyPoint(-0.10,  0.13));
    shape.add(new KeyPoint(-0.15,  0.15));
    shape.add(new KeyPoint(-0.13,  0.10));
    shape.add(new KeyPoint(-0.13, -0.10));
    
    // Instanciate the Locomotion engine (Mobile inherit from Locomotion to implement display functions)
    fLocoEngine = new Mobile("", shape,fWheelDiameter, fWheelDistanceToCenter, fEncoderTickPerRotation, fConvMeterToPix);
    
    // JM : Uncomment to disable angle or distance control
    //fLocoEngine.disableAngleControl(true, 0.);
    //fLocoEngine.disableDistanceControl(true, 0.);
    
    // Define start position
    KeyPoint startpos = new KeyPoint();
    startpos.x = startposx;
    startpos.y = startposx;
    startpos.a = startposa;
    fLocoEngine.setPosition(startpos);
    fLocoEngine.setTarget(startpos);
  }

  protected void finalize() throws Throwable
  {
    fMotorLeft.close();
    fMotorRight.close();
    if( fTwi != null)
      fTwi.close();
    super.finalize(); //not necessary if extending Object.
  } 
  
  /**
   * hardware initialization (real or simulated)
   */
  public void initIOIO(boolean isSimulation, IOIO ioio){

    if( isSimulation ){
    	
      fMotorVirtualLeft = new GenericVirtualMotor(fMaxRealSpeedinTickPerSecond);
      fMotorVirtualRight = new GenericVirtualMotor(fMaxRealSpeedinTickPerSecond);
      fLocoEngine.setMotors(fMotorVirtualLeft, fMotorVirtualRight);      
    
    }else{
    	
      try {
        fTwi = ioio.openTwiMaster(1 /*fI2CIoioNumber*/, TwiMaster.Rate.RATE_400KHz, true);
        fMotorLeft = new IoioMotorControler(  10     /* PwmPin number*/, 
                                              11     /* SignPin number*/, 
                                              0x18   /*i2CDeviceAdress*/, ioio, fTwi,
                                              fMaxRealSpeedinTickPerSecond);
        fMotorRight = new IoioMotorControler( 12     /* PwmPin number*/,
                                              13     /* SignPin number*/, 
                                              0x05   /*i2CDeviceAdress*/, ioio, fTwi,
                                              fMaxRealSpeedinTickPerSecond);
      } catch (ConnectionLostException e) {
        Log.e("ModLoco", e.toString());
      }
      fLocoEngine.setMotors(fMotorLeft, fMotorRight);
    }
    fLocoEngine.setMaxSpeed( 1. );
    fIoioInit = true;
    
  }
  

  /**
   * Main update function.
   */
  public void updateIOIO(){
  	
    if(fLocoEngine == null) return;
    
    
    fLocoEngine.update();
    
    // Notify observer
    setChanged();
    notifyObservers(new Object[]{ "currentPosition", new Object[]{fLocoEngine.getPosition()}});
    
    if( fLastTargetReached != fLocoEngine.isTargetReached())
    {
      setChanged();
      notifyObservers(new Object[]{ "targetReached", new Object[]{fLocoEngine.isTargetReached()}});
      
      fLastTargetReached = fLocoEngine.isTargetReached();
    }
  }

  /**
   * Call on view initialization.
   */
  public void onInitView(Context context){
  }
  
  public void disableAngleControl(boolean disable, double valueWhenDisable)
  {
    fLocoEngine.disableAngleControl(disable, valueWhenDisable);
  }

  public void disableDistanceControl(boolean disable, double valueWhenDisable)
  {
    fLocoEngine.disableDistanceControl(disable, valueWhenDisable);
  }

  public void setSpeedMotorPid(double P, double I, double D)
  {
    fMotorLeft.setPid(P, I, D);
    fMotorRight.setPid(P, I, D);
  }
  
  public void setAnglePid(double P, double I, double D)
  {
    fLocoEngine.setAnglePID(P, I, D);
  }

  public void setDistancePid(double P, double I, double D)
  {
    fLocoEngine.setDistancePID(P, I, D);
  }

  public void setTargetAndPrecision(KeyPoint t, double precision){
  	fLocoEngine.setDistancePrecision(precision);
  	setTarget( t );
  }

  public void setTarget(KeyPoint t){
  	fLocoEngine.setTarget( t );
    setChanged();
    fLastTargetReached = false;
    notifyObservers(new Object[]{ "targetReached", new Object[]{false}});
  }

  public void setTarget(double x, double y, double angle){
    setTarget( new KeyPoint( x,y,angle,0) );
  }
  
  public Object[] getPosition(){
    KeyPoint p = fLocoEngine.getPosition();
    Object [] res = new Object[3];
    res[0] = p.x;
    res[1] = p.y;
    res[2] = p.a;
    return res;
  }
  
  /**
   * Call by the onTouch callback of the view.
   */
  public boolean onTouch(MotionEvent event){
    /*if(fLocoEngine == null) return true;
    float touchX = (int) event.getX();
    float touchY = (int) event.getY();
    KeyPoint pos = fLocoEngine.getPosition();

    Log.i("RobotView", "old pos: " + Double.toString(pos.x) +" , "+ Double.toString(pos.y));
    Log.i("RobotView", "new pos: " +  Float.toString(touchX) +" , "+ Float.toString(touchY));
    
    fLocoEngine.setDistancePrecision(0.1);
    fLocoEngine.setTarget(new KeyPoint(
              touchX * fConvPixToMeter, 
              touchY * fConvPixToMeter
          ));
*/
    return true;
  }

  public void drawNoObstacleShape(Canvas canvas, Paint paint, Mat probaImg, int zoomFactor){
    if(fLocoEngine == null) return;
    fLocoEngine.drawNoObstacleShape(canvas, paint, probaImg, zoomFactor);
  }


  /**
   * Call to refresh the main view draw.
   */
  public void onDraw(Canvas canvas, Paint robotPaint){
    String statusString = "ModLoco ok";
    robotPaint.setColor(Color.GREEN);
    if( !fIoioInit ){
      robotPaint.setColor(Color.RED);
      statusString = "ModLoco not init ";
    }else{
      if(fLocoEngine == null){
        robotPaint.setColor(Color.GRAY);
        statusString = "ModLoco engine nok";
      }
    }

    final int connectedPosY = 473 - 10; // position in the screen
    final int connectedPosX = 685;
    canvas.drawCircle(connectedPosX, connectedPosY, 3, robotPaint);
    canvas.drawText(statusString, connectedPosX+10, connectedPosY+5, robotPaint);

    if(fLocoEngine == null || !fIoioInit) return;
    fLocoEngine.onDraw(canvas, robotPaint);
  }

	@Override
	public void finish() {
		// TODO Auto-generated method stub
		
	}
}
